#include <mc_path_planner/path_smoother/dubins_curve.h>

using namespace std;

double fmodr( double x, double y){
    return x - y*floor(x/y);
}

double mod2pi( double theta ){
    return fmodr( theta, 2 * M_PI );
}

int dubins_LSL(DubinsIntermediateResults* in, vector<double>& out) {
    double tmp0, tmp1, p_sq;
    
    tmp0 = in->d + in->sa - in->sb;
    p_sq = 2 + in->d_sq - (2*in->c_ab) + (2 * in->d * (in->sa - in->sb));

    if(p_sq >= 0) {
        tmp1 = atan2( (in->cb - in->ca), tmp0 );
        out[0] = mod2pi(tmp1 - in->alpha);
        out[1] = sqrt(p_sq);
        out[2] = mod2pi(in->beta - tmp1);
        return EDUBOK;
    }
    return EDUBNOPATH;
}

int dubins_RSR(DubinsIntermediateResults* in, vector<double>& out){
    double tmp0 = in->d - in->sa + in->sb;
    double p_sq = 2 + in->d_sq - (2 * in->c_ab) + (2 * in->d * (in->sb - in->sa));
    if( p_sq >= 0 ) {
        double tmp1 = atan2( (in->ca - in->cb), tmp0 );
        out[0] = mod2pi(in->alpha - tmp1);
        out[1] = sqrt(p_sq);
        out[2] = mod2pi(tmp1 -in->beta);
        return EDUBOK;
    }
    return EDUBNOPATH;
}

int dubins_LSR(DubinsIntermediateResults* in, vector<double>& out){
    double p_sq = -2 + (in->d_sq) + (2 * in->c_ab) + (2 * in->d * (in->sa + in->sb));
    if( p_sq >= 0 ) {
        double p    = sqrt(p_sq);
        double tmp0 = atan2( (-in->ca - in->cb), (in->d + in->sa + in->sb) ) - atan2(-2.0, p);
        out[0] = mod2pi(tmp0 - in->alpha);
        out[1] = p;
        out[2] = mod2pi(tmp0 - mod2pi(in->beta));
        return EDUBOK;
    }
    return EDUBNOPATH;
}

int dubins_RSL(DubinsIntermediateResults* in, vector<double>& out){
    double p_sq = -2 + in->d_sq + (2 * in->c_ab) - (2 * in->d * (in->sa + in->sb));
    if( p_sq >= 0 ) {
        double p    = sqrt(p_sq);
        double tmp0 = atan2( (in->ca + in->cb), (in->d - in->sa - in->sb) ) - atan2(2.0, p);
        out[0] = mod2pi(in->alpha - tmp0);
        out[1] = p;
        out[2] = mod2pi(in->beta - tmp0);
        return EDUBOK;
    }
    return EDUBNOPATH;
}

int dubins_RLR(DubinsIntermediateResults* in, vector<double>& out){
    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sa - in->sb)) / 8.;
    double phi  = atan2( in->ca - in->cb, in->d - in->sa + in->sb );
    if( fabs(tmp0) <= 1) {
        double p = mod2pi((2*M_PI) - acos(tmp0) );
        double t = mod2pi(in->alpha - phi + mod2pi(p/2.));
        out[0] = t;
        out[1] = p;
        out[2] = mod2pi(in->alpha - in->beta - t + mod2pi(p));
        return EDUBOK;
    }
    return EDUBNOPATH;
}

int dubins_LRL(DubinsIntermediateResults* in, vector<double>& out){
    double tmp0 = (6. - in->d_sq + 2*in->c_ab + 2*in->d*(in->sb - in->sa)) / 8.;
    double phi = atan2( in->ca - in->cb, in->d + in->sa - in->sb );
    if( fabs(tmp0) <= 1) {
        double p = mod2pi( 2*M_PI - acos( tmp0) );
        double t = mod2pi(-in->alpha - phi + p/2.);
        out[0] = t;
        out[1] = p;
        out[2] = mod2pi(mod2pi(in->beta) - in->alpha -t + mod2pi(p));
        return EDUBOK;
    }
    return EDUBNOPATH;
}


int dubins_word(DubinsIntermediateResults* in, DubinsPathType pathType, vector<double>& out) {
    int result;
    switch(pathType){
        case LSL:
            result = dubins_LSL(in, out);
            break;
        case RSL:
            result = dubins_RSL(in, out);
            break;
        case LSR:
            result = dubins_LSR(in, out);
            break;
        case RSR:
            result = dubins_RSR(in, out);
            break;
        case LRL:
            result = dubins_LRL(in, out);
            break;
        case RLR:
            result = dubins_RLR(in, out);
            break;
        default:
            result = EDUBNOPATH;
    }
    return result;
}


int dubins_intermediate_results(DubinsIntermediateResults* in, vector<double> start, vector<double> end, double rho){
    double dx, dy, D, d, theta, alpha, beta;
    if( rho <= 0.0 ) {
        return EDUBBADRHO;
    }

    dx = end[0] - start[0];
    dy = end[1] - start[1];
    D = sqrt( dx * dx + dy * dy );
    d = D / rho;
    theta = 0;

    // cout << "dx: " << dx << ", dy: " << dy 
    //     << ", D: " << D << ", d: " << d << ", theta: " << theta
    //     << endl;

    /* test required to prevent domain errors if dx=0 and dy=0 */
    if(d > 0) {
        theta = mod2pi(atan2( dy, dx ));
    }
    alpha = mod2pi(start[2] - theta);
    beta  = mod2pi(end[2] - theta);
    // cout << "alpha: " << alpha << ", beta: " << beta 
    //     << ", theta: " << theta
    //     << endl;

    in->alpha = alpha;
    in->beta  = beta;
    in->d     = d;
    in->sa    = sin(alpha);
    in->sb    = sin(beta);
    in->ca    = cos(alpha);
    in->cb    = cos(beta);
    in->c_ab  = cos(alpha - beta);
    in->d_sq  = d * d;

    return EDUBOK;
}

double dubins_path_length( DubinsPath& path){
    double length = 0.;
    length += path.param[0];
    length += path.param[1];
    length += path.param[2];
    length = length * path.rho;
    return length;
}

void dubins_segment(double t, vector<double>& qi, vector<double>& qt, SegmentType type){
    double st = sin(qi[2]);
    double ct = cos(qi[2]);
    if( type == L_SEG ) {
        qt[0] = +sin(qi[2]+t) - st;
        qt[1] = -cos(qi[2]+t) + ct; 
        qt[2] = t;
    }
    else if( type == R_SEG ) {
        qt[0] = -sin(qi[2]-t) + st;
        qt[1] = +cos(qi[2]-t) - ct;
        qt[2] = -t;
    }
    else if( type == S_SEG ) {
        qt[0] = ct * t;
        qt[1] = st * t;
        qt[2] = 0.0;
    }
    qt[0] += qi[0];
    qt[1] += qi[1];
    qt[2] += qi[2];
}

int dubins_path_sample( DubinsPath& path, double t, vector<double>& q){
    /* tprime is the normalised variant of the parameter t */
    double tprime = t / path.rho;
    vector<double> qi(3, 0.0); /* The translated initial configuration */
    vector<double> q1(3, 0.0); /* end-of segment 1 */
    vector<double> q2(3, 0.0); /* end-of segment 2 */
    const SegmentType* types = DIRDATA[path.type];
    double p1, p2;

    if( t < 0 || t > dubins_path_length(path) ) {
        return EDUBPARAM;
    }

    /* initial configuration */
    qi[0] = 0.0;
    qi[1] = 0.0;
    qi[2] = path.qi[2];

    /* generate the target configuration */
    p1 = path.param[0];
    p2 = path.param[1];
    dubins_segment( p1,      qi,    q1, types[0] );
    dubins_segment( p2,      q1,    q2, types[1] );
    if( tprime < p1 ) {
        dubins_segment( tprime, qi, q, types[0] );
    }
    else if( tprime < (p1+p2) ) {
        dubins_segment( tprime-p1, q1, q,  types[1] );
    }
    else {
        dubins_segment( tprime-p1-p2, q2, q,  types[2] );
    }

    /* scale the target configuration, translate back to the original starting point */
    q[0] = q[0] * path.rho + path.qi[0];
    q[1] = q[1] * path.rho + path.qi[1];
    q[2] = mod2pi(q[2]);

    return EDUBOK;
}


// 曲线数据生成
int DubinPath(vector<double> start, vector<double> end, double rho, vector<vector<double>>& poses){
    int i, errcode;
    DubinsIntermediateResults in;
    errcode = dubins_intermediate_results(&in, start, end, rho);
    if(errcode != EDUBOK) {
        return errcode;
    }

    vector<double> params(3, 0.0);
    double cost;
    double best_cost = INFINITY;
    int best_word = -1;   

    DubinsPath path;
    path.qi[0] = start[0];
    path.qi[1] = start[1];
    path.qi[2] = start[2];
    path.rho = rho;

    for( i = 0; i < 6; i++ ) {
        DubinsPathType pathType = (DubinsPathType)i;
        // cout << "pathType: " << pathType << std::endl;
        errcode = dubins_word(&in, pathType, params);
        // std::cout << "i: " << i <<  ", errcode: " << errcode << std::endl;
        if(errcode == EDUBOK) {
            cost = params[0] + params[1] + params[2];
            // std::cout << "i: " << i <<  ", params[0]: " << params[0] 
            //     << ", params[1]: " << params[1] 
            //     << ", params[2]: " << params[2]
            //     << ", cost: " << cost << std::endl;
            if(cost < best_cost) {
                best_word = i;
                best_cost = cost;
                path.param[0] = params[0];
                path.param[1] = params[1];
                path.param[2] = params[2];
                path.type = pathType;
            }
        }
    }
    if(best_word == -1) {
        return EDUBNOPATH;
    }

    double stepSize = 0.1;
    double length = dubins_path_length(path);
    // cout << "length: " << length << std::endl;
    double x = 0.0;
    vector<double> point(3, 0.0);
    while(x <  length){
        dubins_path_sample(path, x, point);
        poses.emplace_back(point);
        // cout << "x: " << x 
        //     << ", point[0]: " << point[0]
        //     << ", point[1]: " << point[1] 
        //     << ", point[2]: " << point[2]
        //     << std::endl;
        x += stepSize;
    }

    return EDUBOK;
}